#ifndef MOTOR_H
#define MOTOR_H

#include <WProgram.h>
#include <inttypes.h>

#define ID_SPEED_R       5
#define ID_SPEED_L       6
#define ID_DIRECTION_R   7
#define ID_DIRECTION_L   8

// line detect
#define NO_LINE    0
#define LEFT_LINE  1
#define RIGHT_LINE 2
#define FRONT_LINE 3

//motor constrains
#define DELAY_STOP      1000
#define DELAY_DRIVE     1000



#define FORWARD  1
#define BACKWARD 0

class Motor
{
public:
	Motor();
	void drive(int, int, int, int, unsigned long);
	void stopDrive(unsigned long);
        void rotate(int,int,unsigned long);

private:
	int _maxSpeed;
	
};

Motor::Motor()
{
	pinMode(ID_DIRECTION_L, OUTPUT);
	digitalWrite(ID_DIRECTION_L, HIGH);

	pinMode(ID_DIRECTION_R, OUTPUT);
	digitalWrite(ID_DIRECTION_R, HIGH);

	analogWrite(ID_SPEED_L, 0);
	analogWrite(ID_SPEED_R, 0);
	
	_maxSpeed = 255;
}

void Motor::drive(int speedL, int speedR, int directionL, int directionR, unsigned long time = 1)
{
    if(!((digitalRead(ID_DIRECTION_L)==directionL)&&(digitalRead(ID_DIRECTION_R)==directionR)))//Kijken welke richting de wielen draaien, wanneer er 1 van richting is verdraait komt er een delay in
    {
		analogWrite(ID_SPEED_L, 0);
		analogWrite(ID_SPEED_R, 0);
		delay(1);
		digitalWrite(ID_DIRECTION_L, directionL);
		digitalWrite(ID_DIRECTION_R, directionR);
		delay(1);
    }
    //stel in de snelheid
	analogWrite(ID_SPEED_L, speedL);
	analogWrite(ID_SPEED_R, speedR);
	
	delay(time);
}

void Motor::stopDrive(unsigned long time =1)
{
	analogWrite(ID_SPEED_L, 0);
	analogWrite(ID_SPEED_R, 0);
	delay(1);
	digitalWrite(ID_DIRECTION_L, HIGH);
	digitalWrite(ID_DIRECTION_R, HIGH);
	delay(time);
}

void Motor::rotate(int _speed, int direct, unsigned long time=1)
{
  if(direct == 0)
  {
    drive(_speed,_speed,0,1,time);
  }
  else if(direct == 1)
  {
    drive(_speed,_speed,1,0,time);
  }
}
#endif
